#include "rclcpp/rclcpp.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "tf2_ros/transform_broadcaster.h"

int main(int argc, char * argv[])
{
    // 初始化 ROS 2 系统
    rclcpp::init(argc, argv);

    // 创建 ROS 2 节点
    rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("tf_broadcaster_node");

    // 创建 tf2 广播器
    tf2_ros::TransformBroadcaster broadcaster(*node);

    // 设置定时器来定期广播变换
    rclcpp::WallRate loop_rate(1);  // 每秒 1 次

    while (rclcpp::ok())
    {
        geometry_msgs::msg::TransformStamped transformStamped;

        // 设置变换的头部信息
        transformStamped.header.stamp = node->get_clock()->now();
        transformStamped.header.frame_id = "world";   // 父坐标系
        transformStamped.child_frame_id = "robot";    // 子坐标系

        // 设置变换的平移部分
        transformStamped.transform.translation.x = 1.0;
        transformStamped.transform.translation.y = 2.0;
        transformStamped.transform.translation.z = 0.0;

        // 设置变换的旋转部分
        transformStamped.transform.rotation.x = 0.0;
        transformStamped.transform.rotation.y = 0.0;
        transformStamped.transform.rotation.z = 0.0;
        transformStamped.transform.rotation.w = 1.0;

        // 广播变换
        broadcaster.sendTransform(transformStamped);

        rclcpp::spin_some(node);  // 处理回调
        loop_rate.sleep();  // 等待下一次循环
    }

    rclcpp::shutdown();
    return 0;
}
